package com.lego.jbv;

import javax.microedition.sensor.SensorListener;

import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.SensorPort;

public class LigthDark {
	private static LightSensor sensorLeft;
	private static LightSensor sensorRight;
	private static double minPower = 44 ;
	private static double maxPower = 100;
	private static double leftPower = 0;
	private static double rightPower = 0;
	private static int offset = 38;
	private static int maxLeftLight;
	private static int minLeftLight;
	private static int maxRightLight;
	private static int minRightLight;
	private static int SAMPEL_NUMBERS = 10;
	private static int leftLigthArray[] = new int[SAMPEL_NUMBERS];
	private static int rightightArray[] = new int[SAMPEL_NUMBERS];
	private static int counter;
	
	private static void modifyMotorPower(int inputLeft, int inputRight)
	{
		leftPower = (((double)(inputLeft/100d))*(maxLeftLight-minLeftLight))+minLeftLight;
		rightPower = (((double)(inputRight/100d))*(maxRightLight-minRightLight))+minRightLight;
	}
	
	private static void lightLevel()
	{
		maxLeftLight = 0;
		minLeftLight = 65535;
		maxRightLight = 0;
		minRightLight = 65535;
		for(int i = 0; i < SAMPEL_NUMBERS; i++)
		{
			if(maxLeftLight < leftLigthArray[i])
				maxLeftLight = leftLigthArray[i];
			
			if(minLeftLight > leftLigthArray[i])
				minLeftLight = leftLigthArray[i];
			
			if(maxRightLight < rightightArray[i])
				maxRightLight = rightightArray[i];
			
			if(minRightLight > rightightArray[i])
				minRightLight = rightightArray[i];
		}
	}
	
	private static void initLight()
	{
		for(int i = 0; i < SAMPEL_NUMBERS; i++)
		{
			leftLigthArray[i] = sensorLeft.readValue();
			rightightArray[i] = sensorRight.readValue();
			try {
				Thread.sleep(50);
			} catch (Exception e) {
				// TODO: handle exception
			}
		}
	}
	
	
	
	public static void main(String[] args){
		sensorLeft = new LightSensor(SensorPort.S3, false);
		sensorRight = new LightSensor(SensorPort.S4, false);

		counter = 0;
		
		for(int i = 0; i < SAMPEL_NUMBERS; i++)
		{
			leftLigthArray[i] = 0;
			rightightArray[i] = 0;
		}
		
		initLight();
		
		Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {
			    System.exit(0);				
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				
			}
		});
		
		LCD.drawString("LigthDark",0,0);
		LCD.drawString("Left", 0, 1);
		LCD.drawString("Right", 0, 2);
		LCD.drawString("L min", 0, 3);
		LCD.drawString("L max", 0, 4);
		LCD.drawString("R min", 0, 5);
		LCD.drawString("R max", 0, 6);
		while(true)
		{
			if(counter == SAMPEL_NUMBERS)
				counter = 0;
			
			leftLigthArray[counter] = sensorLeft.readValue();
			rightightArray[counter] = sensorRight.readValue();
			
			lightLevel();
			
			modifyMotorPower(leftLigthArray[counter], rightightArray[counter++]);
			
			LCD.drawInt((int)leftPower, 6, 1);
			LCD.drawInt((int)rightPower, 6, 2);
			LCD.drawInt((int)minLeftLight, 6, 3);
			LCD.drawInt((int)maxLeftLight, 6, 4);
			LCD.drawInt((int)minRightLight, 6, 5);
			LCD.drawInt((int)maxRightLight, 6, 6);
			Car.drive((int)(leftPower+minPower), (int)(rightPower+minPower));
		}	
		
		
		
		
	}
}
